#!/bin/bash

CONST_TIMEOUT=20

if false; then
  zenity --warning --title="Example Warning" --text="Example Text" --ok-label="I accept the risks" --extra-button="I decline the risks" --no-wrap --width=400 --height=120
  response=$?
  if [ $response -ne 0 ]; then
      echo "User has declined the risks. Aborting script."
      exit 1
  fi
fi

terminator --geometry "656x400+230+0" -T 'Launching server container...' -e "/home/user/.shadow_launcher_app_dexterous_hand/shadow_hand_launcher/shadow_server_container.sh"

elapsed_time=0
start_time=$SECONDS
proceed=false
while [ $elapsed_time -lt $CONST_TIMEOUT ]; do
  elapsed_time=$(( SECONDS - start_time ))
  if [ "$(docker ps -q -f name=dexterous_hand)" ]; then
    proceed=true
    break;
  fi
done

if [ "$proceed" == false ]; then
    zenity --error --text="Teleop server container not running, execution stopped"
    docker stop dexterous_hand
    exit 1
fi

terminator --geometry "656x400+230+458" -T 'Server ROS Core' -e "/home/user/.shadow_launcher_app_dexterous_hand/shadow_hand_launcher/shadow_roscore.sh"

elapsed_time=0
start_time=$SECONDS
proceed=false
while [ $elapsed_time -lt $CONST_TIMEOUT ]; do
  elapsed_time=$(( SECONDS - start_time ))
  if [[ "$(docker exec -ti dexterous_hand bash -c "source /home/user/projects/shadow_robot/base/devel/setup.bash;rostopic list")" = */rosout* ]]; then
    proceed=true
    break;
  fi
done

if [ "$proceed" == false ]; then
    zenity --error --text="Roscore not running, execution stopped"
    docker stop dexterous_hand
    exit 1
fi

terminator --geometry "656x400+886+0" -T 'NUC Right Hand hardware control loop' -e "/home/user/.shadow_launcher_app_dexterous_hand/shadow_hand_launcher/shadow_nuc_right_hardware_control_loop.sh 1"
sleep 30 # temporary workaround until arm controllers starting on time is fixed
terminator --geometry "656x400+886+458" -T 'Server Right Hand GUI' -e "/home/user/.shadow_launcher_app_dexterous_hand/shadow_hand_launcher/shadow_GUI_right.sh"
